using System;
using System.Collections.Generic;
using System.Text;

namespace OpenCvSharp
{
    public static class CvMeasure
    {
        public static List<CvPoint> MeasurePos(Mat image, CvRotatedRect caliper, double smooth = 1d, double threshold = 30d, CvTransition transition = CvTransition.Positive, CvSelect select = CvSelect.Max)
        {
            CvRect roi = new CvRect(caliper.CenterX - caliper.Width / 2d, caliper.CenterY - caliper.Height / 2d, caliper.Width, caliper.Height);
            Rect cutRoi = new Rect((int)roi.X, (int)roi.Y, (int)roi.Width, (int)roi.Height);
            double xErr = roi.X - cutRoi.X + roi.X;
            Mat src;
            if (caliper.Angle != 0d)
            {
                using (Mat matrix = Cv2.GetRotationMatrix2D(caliper.Center, caliper.Angle, 1d))
                {
                    using (Mat rImg = image.WarpAffine(matrix, image.Size(), InterpolationFlags.Linear))
                    {
                        src = rImg.Clone(cutRoi);
                    }
                }
            }
            else
            {
                src = image.Clone(cutRoi);
            }

            double[] gradient;
            using (Mat reduce = new Mat())
            using (Mat gauss = new Mat())
            using (Mat normalize = new Mat())
            using (Mat edges = new Mat())
            {
                Cv2.Reduce(src, reduce, ReduceDimension.Row, ReduceTypes.Avg, MatType.CV_64F);
                Cv2.GaussianBlur(reduce, gauss, Size.Zero, smooth, 0d, BorderTypes.Default);
                Cv2.Normalize(gauss, normalize, 0d, threshold, NormTypes.MinMax, -1, null);
                Cv2.Sobel(normalize, edges, MatType.CV_64F, 1, 0, 3, 1d, 0d, BorderTypes.Default);
                src.Dispose();
                WriteMat(reduce, gauss, normalize, edges);
                edges.GetArray<double>(out gradient);
            }

            List<CvPoint> points = new List<CvPoint>(gradient.Length);
            bool loop = true;
            bool s1, s2;
            CvPoint p;
            for (int i = 3, imax = 0; loop && i < gradient.Length - 3; i++) //不会找幅度顶点，待有缘人解决
            {
                if (Math.Abs(gradient[i]) < threshold)
                {
                    continue;
                }

                s1 = gradient[i] < gradient[i - 3];
                s1 &= gradient[i] < gradient[i - 2];
                s1 &= gradient[i] < gradient[i - 1];
                s1 &= gradient[i] < gradient[i + 1];
                s1 &= gradient[i] < gradient[i + 2];
                s1 &= gradient[i] < gradient[i + 3];

                s2 = gradient[i] > gradient[i - 3];
                s2 &= gradient[i] > gradient[i - 2];
                s2 &= gradient[i] > gradient[i - 1];
                s2 &= gradient[i] > gradient[i + 1];
                s2 &= gradient[i] > gradient[i + 2];
                s2 &= gradient[i] > gradient[i + 3];

                if (!s1 && !s2)
                {
                    continue;
                }

                switch (transition)
                {
                    case CvTransition.Positive:
                        if (s1)
                        {
                            continue;
                        }
                        break;
                    case CvTransition.Negative:
                        if (s2)
                        {
                            continue;
                        }
                        break;
                    case CvTransition.All:
                    default:
                        break;
                }
                switch (select)
                {
                    case CvSelect.First:
                        if (points.Count > 0)
                        {
                            loop = false;
                            continue;
                        }
                        break;
                    case CvSelect.Last:
                        if (points.Count > 0)
                        {
                            points.Clear();
                        }
                        break;
                    case CvSelect.Max:
                        if (points.Count > 0)
                        {
                            if (Math.Abs(gradient[i]) > Math.Abs(gradient[imax]))
                            {
                                points.Clear();
                                imax = i;
                            }
                            else
                            {
                                continue;
                            }
                        }
                        else
                        {
                            imax = i;
                        }
                        break;
                    //case CvSelect.All:
                    default:
                        break;
                }

                p = CalcSubPixelPoint(i - 1, gradient[i - 1], i, gradient[i], i + 1, gradient[i + 1]);
                p.X += xErr;
                p.Y = caliper.CenterY;
                points.Add(p);
            }
            if (points.Count < 1)
            {
                return new List<CvPoint>();
            }
            List<CvPoint> result;
            if (caliper.Angle != 0d)
            {
                result = new List<CvPoint>(points.Count);
                using (Mat matrix = Cv2.GetRotationMatrix2D(caliper.Center, -caliper.Angle, 1d))
                {
                    Cv2.Transform(InputArray.Create(points, MatType.CV_64FC2), OutputArray.Create(result), matrix);
                }
            }
            else
            {
                result = points;
            }
            return result;
        }

        private static void WriteMat(params Mat[] mat)//测试用的方法，可以删除调用
        {
            using (System.IO.StreamWriter sw = new System.IO.StreamWriter("1.csv", false, Encoding.UTF8))
            {
                MatType type;
                for (int i = 0; i < mat.Length; i++)
                {
                    type = mat[i].Type();
                    for (int r = 0, rc = mat[i].Rows; r < rc; r++)
                    {
                        for (int c = 0, cc = mat[i].Cols; c < cc; c++)
                        {
                            switch (type.Depth)
                            {
                                case MatType.CV_8U:
                                    sw.Write(mat[i].Get<byte>(r, c));
                                    break;
                                case MatType.CV_8S:
                                    sw.Write(mat[i].Get<sbyte>(r, c));
                                    break;
                                case MatType.CV_16U:
                                    sw.Write(mat[i].Get<ushort>(r, c));
                                    break;
                                case MatType.CV_16S:
                                    sw.Write(mat[i].Get<short>(r, c));
                                    break;
                                case MatType.CV_32S:
                                    sw.Write(mat[i].Get<int>(r, c));
                                    break;
                                case MatType.CV_32F:
                                    sw.Write(mat[i].Get<float>(r, c));
                                    break;
                                case MatType.CV_64F:
                                    sw.Write(mat[i].Get<double>(r, c));
                                    break;
                                default:
                                    break;
                            }
                            sw.Write(",");
                        }
                        sw.WriteLine();
                    }
                    sw.WriteLine();
                }
            }

        }

        private static CvPoint CalcSubPixelPoint(double x1, double y1, double x2, double y2, double x3, double y3)//三点算亚像素，暂时没有想到其它办法
        {
            double denom = (x1 - x2) * (x1 - x3) * (x2 - x3);
            double a = (x3 * (y2 - y1) + x2 * (y1 - y3) + x1 * (y3 - y2)) / denom;
            double b = (x3 * x3 * (y1 - y2) + x2 * x2 * (y3 - y1) + x1 * x1 * (y2 - y3)) / denom;
            double c = (x2 * x3 * (x2 - x3) * y1 + x3 * x1 * (x3 - x1) * y2 + x1 * x2 * (x1 - x2) * y3) / denom;
            return new CvPoint(-b / (a * 2d), c - b * b / (a * 4d));
        }

        public static CvLine FindLine(Mat image, CvRotatedRect searchArea, int caliperNum = 10, double caliperWidth = 10d, double smooth = 1d, double threshold = 30d, CvTransition transition = CvTransition.Positive, CvSelect select = CvSelect.Max,
            List<CvDispObj> resultPoints = null, List<CvDispObj> resultCalipers = null)
        {
            searchArea.Angle %= 360d;
            if (searchArea.Width < caliperWidth)
            {
                caliperWidth = searchArea.Width;
            }

            double caliperDis = searchArea.Height  / (caliperNum - 1);
            double rad = CvOperator.ToRad(searchArea.Angle+90d);
            double sin = Math.Sin(rad);
            double cos = Math.Cos(rad);
            double beginX = searchArea.CenterX + (searchArea.Height / 2d) * cos;
            double beginY = searchArea.CenterY + (searchArea.Height / 2d) * sin;

            List<CvRotatedRect> caliperList = new List<CvRotatedRect>();
            List<CvPoint> caliperRetsult = new List<CvPoint>();
            for (int i = 0; i < caliperNum; i++)
            {
                caliperList.Add(new CvRotatedRect(beginX + i * caliperDis * -cos, beginY + i * caliperDis * -sin, searchArea.Angle, searchArea.Width, caliperWidth));
                caliperRetsult.AddRange(MeasurePos(image, caliperList[i], smooth, threshold, transition, select));
            }

            CvLine result = new CvLine(double.NaN, double.NaN, double.NaN, double.NaN);
            switch (select)
            {
                case CvSelect.First:
                case CvSelect.Last:
                case CvSelect.Max:
                    using (Mat fitLine = new Mat()) //拟合算法待新增优化，暂时只会用默认的
                    {
                        Cv2.FitLine(InputArray.Create(caliperRetsult, MatType.CV_64FC2), OutputArray.Create(fitLine), DistanceTypes.Huber, 0d, 0.01, 0.01);
                        Vec4d line2d = new Vec4d(fitLine.Get<float>(0, 0), fitLine.Get<float>(1, 0), fitLine.Get<float>(2, 0), fitLine.Get<float>(3, 0));
                        line2d[0] *= searchArea.Height / 2d;
                        line2d[1] *= searchArea.Height / 2d;
                        result.BeginX = line2d[2] - line2d[0];
                        result.BeginY = line2d[3] - line2d[1];
                        result.EndX = line2d[2] + line2d[0];
                        result.EndY = line2d[3] + line2d[1];
                    }
                    break;
                //case CvSelect.All:
                default:
                    break;
            }

            if (resultPoints != null)
            {
                for (int i = 0; i < caliperRetsult.Count; i++)
                {
                    resultPoints.Add(new CvDispObj.CvDispCross(caliperRetsult[i]) { Color = System.Drawing.Color.OrangeRed });
                }
            }
            if (resultCalipers != null)
            {
                for (int i = 0; i < caliperList.Count; i++)
                {
                    resultCalipers.Add(new CvDispObj.CvDispRotatedRect(caliperList[i]) { Color = System.Drawing.Color.Blue, DisplayArrow = true, ArrowInsideMode = true });
                }
            }
            return result;
        }

        public static CvCircle FindCircle(Mat image, CvCircle searchArea, double startAngle = 0f, double sweepAngle = 360f, int caliperNum = 10, double caliperWidth = 10d, double caliperLength = 100d, double smooth = 1d, double threshold = 30d, CvDirection direction = CvDirection.Forward, CvTransition transition = CvTransition.Positive, CvSelect select = CvSelect.Max,
            List<CvDispObj> resultPoints = null, List<CvDispObj> resultCalipers = null)
        {
            startAngle %= 360d;
            if (sweepAngle < 0d || sweepAngle > 360d)
            {
                sweepAngle = 360f;
            }

            double caliperDis = sweepAngle / (caliperNum - 1);
            List<CvRotatedRect> caliperList = new List<CvRotatedRect>();
            double tmpX, tmpY, tmpA;
            List<CvPoint> caliperRetsult = new List<CvPoint>();
            for (int i = 0; i < caliperNum; i++)
            {
                tmpA = startAngle + caliperDis * i;
                CvOperator.MovePoint(searchArea.CenterX, searchArea.CenterY, tmpA, searchArea.Radius, out tmpX, out tmpY);
                if (direction == CvDirection.Reverse)
                {
                    tmpA += 180d;
                    tmpA %= 360d;
                }
                caliperList.Add(new CvRotatedRect(tmpX, tmpY, tmpA, caliperLength, caliperWidth));
                caliperRetsult.AddRange(MeasurePos(image, caliperList[i], smooth, threshold, transition, select));
            }

            CvCircle result = new CvCircle(double.NaN, double.NaN, double.NaN);
            switch (select)
            {
                case CvSelect.First:
                case CvSelect.Last:
                case CvSelect.Max:
                    result = CvOperator.FitCircle(caliperRetsult);//和线一样，拟合算法只写了这一种
                    break;
                //case CvSelect.All:
                default:

                    break;
            }
            if (resultPoints != null)
            {
                for (int i = 0; i < caliperRetsult.Count; i++)
                {
                    resultPoints.Add(new CvDispObj.CvDispCross(caliperRetsult[i]) { Color = System.Drawing.Color.OrangeRed });
                }
            }
            if (resultCalipers != null)
            {
                for (int i = 0; i < caliperList.Count; i++)
                {
                    resultCalipers.Add(new CvDispObj.CvDispRotatedRect(caliperList[i]) { Color = System.Drawing.Color.Blue, DisplayArrow = true, ArrowInsideMode = true });
                }
            }
            return result;
        }
    }
}
